/*
 *  Extended Kalman Filter estimator
 *  Copyright (C) 2012, CYPHY lab
 *  Inkyu Sa <i.sa@qut.edu.au>
 *
 *  http://wiki.qut.edu.au/display/cyphy
 *
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef CYPHY_EKF_POLE_H
#define CYPHY_EKF_POLE_H

#include <iostream>
#include <ros/ros.h>
#include <sstream>

/*
#include <mrpt/base.h>
#include <mrpt/utils.h>
#include <mrpt/math.h>
#include <math.h>
*/

//#include "EKF.h"

#include <Eigen/Core>
#include <Eigen/StdVector>
#include <vector>
#include <algorithm>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <Eigen/Eigenvalues>
#include "std_msgs/Float32MultiArray.h"
#include "sensor_msgs/Imu.h"
#include "nav_msgs/Odometry.h"



using namespace std;
using namespace Eigen;

void jac(double P1x,double P1y,double P1z,
         double P2x,double P2y,double P2z,
         double c_x,double c_y,double c_z,
         double fx,double fy,double px,double py,
         double phi,double theta_Var,double psi_Var,
         double (*MatrixWithNoName)[9]);

void line_model(double P1x,double P1y,double P1z,
		     	double P2x,double P2y,double P2z,
			double c_x,double c_y,double c_z,
			double fx,double fy,double px,double py,
			double phi,double theta_Var,double psi_Var,
			double (*MatrixWithNoName)[1]);






class cyphy_ekf_pole
{
	public:

	struct output
	{
		Matrix<float,9,1> y;   //x,y,z,dx,dy,dz,r,p,y
		Matrix<float,9,9> covs;
	};
	cyphy_ekf_pole (ros::NodeHandle nh);
	void cam_imu_EKF(const Matrix<float,1,4>* measured_line,const Matrix<float,1,2>* measured_ang, output* out);
	void poleOutCallback(const std_msgs::Float32MultiArray& msg); 
	void mkImuCallback(const sensor_msgs::ImuConstPtr& imu_msg);
	void poleInitCallback(const std_msgs::Float32MultiArray& pose_msg);
	void ekf_init();
	Matrix<float,4,9> Jac_H(Matrix<float,9,1>x);
	Matrix<float,4,1> LineModel_h(Matrix<float,9,1>x);
	Matrix<float,9,9> Q;
	Matrix<float,6,6> R;
	Matrix<float,9,1> x_hat;
	Matrix<float,9,9> P_hat;
	Matrix<float,9,9> F;
	Matrix<float,2,9> H_I;
	Matrix<float,9,3> B;
	float fx,fy,px,py;


	float dt;
	double deg2rad(double deg);
	double rad2deg(double rad);    
	virtual ~ cyphy_ekf_pole  ();
	std_msgs::Float32MultiArray pole_out_msg;
	std_msgs::Float32MultiArray pole_init_msg;
	sensor_msgs::Imu Imu_msg;
	ros::Subscriber pole_outSubscriber;
	ros::Subscriber mkImuSubscriber;
	ros::Subscriber poleInitSubscriber;
	ros::Publisher pub_ekf_odom;

	Matrix<float, 1, 4> measured_line;
	Matrix<float, 1, 2> measured_ang;
	Matrix<float, 3, 1> measured_gyro;
	
	Matrix<float, 9, 1> y;
	Matrix<float, 9, 9> state_cov;
	output ekf_out;
	bool initialized;
	nav_msgs::Odometry ekf_mk_odom_msg;
	ros::Time now,past;
	bool get_init_pose;


	private:
	// **** ROS-related
	ros::NodeHandle nh_;
	//ros::Subscriber nav_sub;



	//void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_msg);
	//void navCallback(const ardrone_pose_estimation::NavdataConstPtr& nav_msg);
	//void imuCallback (const sensor_msgs::Imu::ConstPtr& imu_msg);
	//bool getBaseToLaserTf (const sensor_msgs::LaserScan::ConstPtr& scan_msg);
	//void createCache (const sensor_msgs::LaserScan::ConstPtr& scan_msg);

};






#endif

